package butines.game2.sterring;

import butines.common.math.Vector2D;
import butines.game2.entity.EntityMoving;

public class Steering {
    
    private final EntityMoving owner;
    
    private Vector2D steeringForce;
    private Vector2D target;

    private boolean seek;
    private boolean arrive;
    private boolean followPath;
    
    private float weightSeek = 1f;
    private float weightArrive = 1f;
    private float weightFollowPath = 1f;
    
    private float slowingDistance = 1;
    private float waypointSeekDistSq = 4;
    
    private Path path;
    
    public Steering(EntityMoving owner) {
        this.owner = owner;
        steeringForce = new Vector2D();
        target = new Vector2D();
        path = new Path();
    }
    
    private Vector2D seek(Vector2D target) {
        Vector2D desiredVelocity = new Vector2D();
        desiredVelocity.sub(target, owner.getPosition()).normalise().mult(owner.getMaxSpeed());
        return desiredVelocity.sub(owner.getVelocity());
    }

    public Vector2D arrive(Vector2D target, float slowingDistance) {

        Vector2D toTarget = new Vector2D();
        Vector2D.sub(toTarget, target, owner.getPosition());
        
        float distance = toTarget.length();
        if (distance > 0) {
            float speed = Math.min(owner.getMaxSpeed() * (distance / slowingDistance), owner.getMaxSpeed());
            Vector2D force = new Vector2D();
            force.mult(toTarget, speed / distance);
            force.sub(owner.getVelocity());
            return force;
        }

        return new Vector2D(0, 0);
    }

    private Vector2D followPath() {
        // move to next target if close enough to current target (working in
        // distance squared space)
        if (path.size() == 0) {
            return new Vector2D();
        }

        if (Vector2D.distanceSq(path.getCurrentWaypoint(), owner.getPosition()) < waypointSeekDistSq) {
            path.nextWaypoint();
        }
        
        if (path.isFinished()) {
            return arrive(path.getCurrentWaypoint(), slowingDistance);
        } else {
            return seek(path.getCurrentWaypoint());
        }
    }
    
    private boolean accumulateForce(Vector2D forceToAdd) {
        //calculate how much steering force the vehicle has used so far
        float magnitudeSoFar = steeringForce.length();
        
        // calculate how much steering force remains to be used by this vehicle
        float magnitudeRemaining = owner.getMaxForce() - magnitudeSoFar;

        // return false if there is no more force left to use
        if (magnitudeRemaining <= 0.0) {
            return false;
        }

        // calculate the magnitude of the force we want to add
        float magnitudeToAdd = forceToAdd.length();

        // if the magnitude of the sum of ForceToAdd and the running total
        // does not exceed the maximum force available to this vehicle, just
        // add together. Otherwise add as much of the ForceToAdd vector is
        // possible without going over the max.
        if (magnitudeToAdd < magnitudeRemaining) {
            steeringForce.add(forceToAdd);
        } else {
            magnitudeToAdd = magnitudeRemaining;
            // add it to the steering force
            Vector2D.multAdd(steeringForce, forceToAdd, magnitudeToAdd);
        }

        return true;
    }
    
    private void calculatePrioritized() {
        Vector2D force;

        if (seek) {
            force = seek(target).mult(weightSeek);
            if (!accumulateForce(force)) {
                return;
            }
        }

        if (arrive) {
            force = arrive(target, slowingDistance).mult(weightArrive);
            if (!accumulateForce(force)) {
                return;
            }
        }
        
        if (followPath) {
            force = followPath().mult(weightArrive);
            if (!accumulateForce(force)) {
                return;
            }
        }
        
    }
    
    public Vector2D calculate() {
        steeringForce.set(0, 0);
        
        calculatePrioritized();
        
        return steeringForce;
    }

    public boolean isSeek() {
        return seek;
    }

    public void setSeek(boolean seek) {
        this.seek = seek;
    }

    public boolean isArrive() {
        return arrive;
    }

    public void setArrive(boolean arrive) {
        this.arrive = arrive;
    }

    public float getWeightSeek() {
        return weightSeek;
    }

    public void setWeightSeek(float weightSeek) {
        this.weightSeek = weightSeek;
    }

    public float getWeightArrive() {
        return weightArrive;
    }

    public void setWeightArrive(float weightArrive) {
        this.weightArrive = weightArrive;
    }
    
    public Vector2D getSteeringForce() {
        return steeringForce;
    }

    public Vector2D getTarget() {
        return target;
    }

    public float getSlowingDistance() {
        return slowingDistance;
    }

    public void setSlowingDistance(float slowingDistance) {
        this.slowingDistance = slowingDistance;
    }

    public boolean isFollowPath() {
        return followPath;
    }

    public void setFollowPath(boolean followPath) {
        this.followPath = followPath;
    }

    public float getWeightFollowPath() {
        return weightFollowPath;
    }

    public void setWeightFollowPath(float weightFollowPath) {
        this.weightFollowPath = weightFollowPath;
    }

    public float getWaypointSeekDistSq() {
        return waypointSeekDistSq;
    }

    public void setWaypointSeekDistSq(float waypointSeekDistSq) {
        this.waypointSeekDistSq = waypointSeekDistSq;
    }

    public Path getPath() {
        return path;
    }
    
}
